/// 计算三维坐标
/// 目前是通过图片验证小球的位置

#include "opencv2/calib3d.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"

#include "CvCalibCommon.h"

#include <vector>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <iterator>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>

using namespace cv;
using namespace std;

///
/// \brief findLightBalls
/// \param grayImage 输入图像
/// \param nThresh
/// \param outPoints
/// \param rMin 半径最小值
///
static void findLightBalls(Mat &grayImage, int nThresh, vector<Point2f> &outPoints, const float rMin = 3.0)
{
    Mat threshold_output;
    vector<vector<Point> >  contours;
    vector<Vec4i> hierarchy;
    // blur
    blur(grayImage,grayImage,Size(3,3));

    // Canny

    // threshold
    threshold(grayImage,threshold_output,nThresh,255,THRESH_BINARY);

    // contours
    findContours(threshold_output,contours,hierarchy,RETR_TREE,CHAIN_APPROX_SIMPLE,Point(0,0));

    vector<vector<Point> > contours_poly(contours.size());
    vector<Point2f> center(contours.size());
    vector<float>radius(contours.size());

    for (uint i = 0;i < contours.size();i++){
        approxPolyDP(Mat(contours[i]),contours_poly[i],3,true);
        minEnclosingCircle(contours_poly[i],center[i],radius[i]);
        if(radius[i] >= rMin) {
            outPoints.push_back(center[i]);
        }
    }

    if(0) {
        Mat drawing = Mat::zeros(threshold_output.size(),CV_8UC3);
        for(int i = 0 ; i < contours.size(); i++){
            Scalar color = Scalar(0, 0, 255);
            //drawContours(drawing,contours_poly,i,color,1,8,vector<Vec4i>(),0,Point());
            //rectangle(drawing,boundRect[i].tl(),boundRect[i].br(),color,2,8,0);
            circle(drawing,center[i],(int)radius[i],color,2,8,0);
            cout << i+1 <<  " circle = " << center[i] << ", " << radius[i] << endl;
        }

        imshow("show",drawing);
    }
}

int main(int argc, char const *argv[])
{
    string inputFolder = "D:/Apps/Aimooe/AimToolbox/DualImage/";
    string baseFolder = "D:/sai/opencv/images/aimdata/";
    string outputFolder = "D:/sai/opencv/images/aimdata/output/";

    Size imageSize = Size(1280,720);
    int nThreshold = 108; // 曝光 1400

    const int nBuf = 1024;
    char buf[nBuf]{0};
    // 1. 读取指定文件里面的图像
    cout << "please input left image name(like C0_1.bmp):";
    fgets(buf, 1024,stdin);
    string imgNameL = inputFolder + buf;
    cout << "Left image file name:" << imgNameL << endl;

    cout << "please input left image name(like C0_1.bmp):";
    fgets(buf, 1024,stdin);
    string imgNameR = inputFolder + buf;
    cout << "Right image file name:" << imgNameR << endl;

    // TODO:
//    imgNameL = "D:/sai/opencv/images/aimdata/balls/C0_1.bmp";
//    imgNameR = "D:/sai/opencv/images/aimdata/balls/C1_1.bmp";

    // 2. 读取参数
    string paramFileName = baseFolder + "params.yml";
    FileStorage fs(paramFileName, FileStorage::READ);
    Mat M1, D1, M2, D2;  // 内参和畸变
    Mat R, T;
    fs["M1"] >> M1;
    fs["D1"] >> D1;
    fs["M2"] >> M2;
    fs["D2"] >> D2;
    fs["R"] >> R;
    fs["T"] >> T;

    // 定义 单位旋转矩阵和0平移矩阵
    Mat R0 = Mat::eye(3,3, CV_64FC1);
    Mat T0 =Mat(3,1,CV_64FC1,Scalar::all(0));

    // 1. 读取原始图像, 如果不是灰度，药转成灰度图像
    Mat img1 = imread(imgNameL, IMREAD_GRAYSCALE);
    Mat img2 = imread(imgNameR, IMREAD_GRAYSCALE);

    // 2. 矫正图像
    Mat map1x, map1y, map2x, map2y;
    initUndistortRectifyMap(M1, D1, R0, M1, imageSize, CV_16SC2, map1x, map1y);
    initUndistortRectifyMap(M2, D2, R0, M2, imageSize, CV_16SC2, map2x, map2y);

    Mat img1R = img1.clone();
    Mat img2R = img2.clone();
    cv::remap( img1, img1R, map1x, map1y, INTER_LINEAR);
    cv::remap( img2, img2R, map2x, map2y, INTER_LINEAR);

    // 3. 记录矫正的结果
    //imwrite(outputFolder + "left01_r.jpg", img1R);
    //imwrite(outputFolder + "right01_r.jpg", img2R);

    // 4. 在矫正的图像中查找数据
    vector<Point2f> outPointsL, outPointsR;
    CvCalibCommon::findLightBalls(img1R, nThreshold, outPointsL, 3.0);
    CvCalibCommon::findLightBalls(img2R, nThreshold, outPointsR, 3.0);

    cout<< "outPointsL: " << outPointsL << endl;
    cout<< "outPointsR: " << outPointsR << endl;

    // 5. 计算三维坐标并确定 是否正确
    vector<Point3f> pointsWorld;
    uint i, j;
    vector<uint> pointsMapped;
    vector<uint>::iterator it;
    for (i = 0; i < outPointsL.size(); i++) {
        for (j=0; j< outPointsR.size(); j++) {
            it = find (pointsMapped.begin(), pointsMapped.end(), j);
            if(it == pointsMapped.end()) {
                Point3f worldPoint = CvCalibCommon::uv2xyz(outPointsL[i], outPointsR[j], M1, R0, T0,
                                                           M2, R, T);
                // 反投影回去，如果不是符合要求的点，反映射回去的点和原来的点不匹配
                Point2f leftPoint = CvCalibCommon::xyz2uv(worldPoint, M1, R0, T0, D1, false);
                Point2f rightPoint = CvCalibCommon::xyz2uv(worldPoint, M2, R, T, D2, false);
                double err = norm(Mat(leftPoint), Mat(outPointsL[i]), NORM_L2) + norm(Mat(rightPoint), Mat(outPointsR[j]), NORM_L2);
                cout << "worldPoint[" << i << "," << j << "]" << worldPoint << ", err:" << err << endl;
                if (err < 1.0) {
                    pointsWorld.push_back(worldPoint);
                    pointsMapped.push_back(j);
                }
            }
        }
    }

    // 6. 打印结果
    cout << "Result Point:" << endl;
    for (i = 0; i< pointsWorld.size(); i++) {
        cout << i+1 << ": " << pointsWorld[i] << endl;
    }

    return 0;
}
